#!/usr/bin/python3
# -*- coding: utf-8 -*-

#publish images
import rospy
from sensor_msgs.msg import Image
import gxipy as gx
from cv_bridge import CvBridge,CvBridgeError
import cv2
import sys

import time
import os

bridge=CvBridge()

device_manager = gx.DeviceManager()
dev_num, dev_info_list = device_manager.update_device_list()
if dev_num == 0:
    sys.exit(1)
'''
每个相机的sn都是独特的，在相机下面写着，这样就可以利用这个分开长短焦相机
sn作为launch文件中外传入的参数
'''

param_list = rospy.get_param_names()
for param in param_list:  #反证只有一个键，虽说跟node有关
    if '/sn' in param:
        sn = rospy.get_param(param) 
    if '/ExposureTime' in param:
        ExposureTime = rospy.get_param(param)
cam = device_manager.open_device_by_sn(sn)
#set parameters
cam.TriggerMode.set(gx.GxSwitchEntry.OFF)#set continuous acquisition
cam.ExposureTime.set(ExposureTime)  #set exposure time


cam.stream_on()

def talker():
    pub = rospy.Publisher('image_dh', Image, queue_size=1)
    rospy.init_node('publisher', anonymous=True)
    while not rospy.is_shutdown():
        raw_image = cam.data_stream[0].get_image()
        numpy_image = raw_image.get_numpy_array()
        if numpy_image is None:
            continue
        msg=bridge.cv2_to_imgmsg(numpy_image,"mono8")   #bgr image
        pub.publish(msg)

        if cv2.waitKey(1) & 0xFF==ord('q'):
            break
        if rospy.is_shutdown():
            cam.stream_off()
            cam.close_device()

        if not os.path.exists("~/bs_gd_single/bag/image_wa"):
            os.makedirs("~/bs_gd_single/bag/image_wa")          
        cv2.imwrite(f"~/bs_gd_single/bag/image_wa/{time.time()}.jpg",numpy_image)

if __name__ == '__main__':
    try:
        talker()
    except rospy.ROSInterruptException:
        pass



# #!/usr/bin/env python
# # -*- coding: utf-8 -*-
# import roslaunch
# import rospy
# import time
# import sys
# from geometry_msgs.msg import Transform,Pose,Point32
# from sensor_msgs.msg import Image
# import gxipy as gx
# from cv_bridge import CvBridge

# import cv2
# import time
# import os
# import numpy as np

# class Wacam_start():
#     def __init__(self) -> None:
#         self.bridge=CvBridge() 
        
#         device_manager = gx.DeviceManager()
#         dev_num, dev_info_list = device_manager.update_device_list()
#         if dev_num == 0:
#             sys.exit(1)

#         param_list = rospy.get_param_names()
#         for param in param_list:  #反证只有一个键，虽说跟node有关
#             if '/sn' in param:
#                 sn = rospy.get_param(param) 
#             if '/ExposureTime' in param:
#                 ExposureTime = rospy.get_param(param)
#         self.cam = device_manager.open_device_by_sn(sn)
#         #set parameters
#         self.cam.TriggerMode.set(gx.GxSwitchEntry.OFF)#set continuous acquisition
#         self.cam.ExposureTime.set(ExposureTime)  #set exposure time
        
#         self.pub = rospy.Publisher('image_dh', Image, queue_size=1)  #话题一直存在，只有当距离足够近时才会发布图像
#         self.sf_drogue_sub = rospy.Subscriber("/bs_debuger/result/sf_drogue", Transform, self.start_wacam)
#         rospy.spin()

    
#     def start_wacam(self,transform_msg):
#         if not(np.isnan(transform_msg.translation.z)):
#             if transform_msg.translation.z!=np.nan and transform_msg.translation.z <= 5:     #这里设的是短焦相机估计锥套距离小于5m时开始启动广角相机
#                 self.cam.stream_on()
#                 while not rospy.is_shutdown():
#                     raw_image = self.cam.data_stream[0].get_image()
#                     numpy_image = raw_image.get_numpy_array()
#                     if numpy_image is None:
#                         continue
#                     msg=self.bridge.cv2_to_imgmsg(numpy_image,"mono8")   #bgr image
#                     self.pub.publish(msg)

#                     # save images
#                     if not os.path.exists("/home/brightsoul/bs_gd_single/bag/image_wa"):
#                         os.makedirs("/home/brightsoul/bs_gd_single/bag/image_wa")
#                     cv2.imwrite(f"/home/brightsoul/bs_gd_single/bag/image_wa/{time.time()}.jpg",numpy_image)
#             else:
#                 self.cam.stream_off()
#                 self.cam.close_device()
                
            
# if __name__ == '__main__':
#     rospy.init_node('start_cam_node', anonymous=True)
#     Wacam_start()


